package com.dji.ux.sample.VirtualSticks;

import android.app.AlertDialog;
import android.content.Context;
import android.content.DialogInterface;
import android.util.Log;
import android.view.LayoutInflater;
import android.view.View;
import android.widget.EditText;

import com.dji.ux.sample.DJISampleApplication;
import com.dji.ux.sample.Utils.MiniPID;

import com.dji.ux.sample.R;
import com.google.gson.Gson;

import org.json.JSONObject;

import java.util.Timer;

import dji.common.error.DJIError;
import dji.common.flightcontroller.FlightControllerState;
import dji.common.flightcontroller.virtualstick.VerticalControlMode;
import dji.common.gimbal.Rotation;
import dji.common.gimbal.RotationMode;
import dji.common.util.CommonCallbacks;
import dji.keysdk.DJIKey;
import dji.keysdk.GimbalKey;
import dji.keysdk.FlightControllerKey;
import dji.keysdk.callback.ActionCallback;
import dji.keysdk.callback.SetCallback;
import dji.sdk.base.BaseProduct;
import dji.sdk.flightcontroller.FlightController;
import dji.sdk.products.Aircraft;
import dji.sdk.sdkmanager.DJISDKManager;

import static com.dji.ux.sample.Utils.SimpleUtils.showToast;

public class VirtualSticksController {

    //Left Stick
    private float verticalJoyControlMaxSpeed = 0.05f;
    private float yawJoyControlMaxSpeed = 3;

    //Right Stick
    private float pitchJoyControlMaxSpeed = 0.01f;
    private float rollJoyControlMaxSpeed = 0.01f;
    public FlightController mflightController;

    private Timer mSendVirtualStickDataTimer;
    private SendVirtualStickDataTask mSendVirtualStickDataTask;
    private float lX = 0, lY = 0, rX = 0, rY = 0;

    private Context mcontext;

    private boolean isLanding = false;
    private boolean isAlternating = false;
    static public boolean readyAlternating = false;
    private float curr_height;
    private String detectType;

//    private PIDUtil pidUtilX;
//    private PIDUtil pidUtilY;

    private MiniPID pidUtilX;
    private MiniPID pidUtilY;

    private String KP_value = "KP";
    private String KI_value = "KI";
    private String KD_value = "KD";

    private float KP_vertical_speed = -2f;
    private boolean isvirtualStickEnable = false;

    static public float landHeading = 0;

    public VirtualSticksController(Context context) {
        if (pidUtilX == null) {
            pidUtilX = new MiniPID(0.04f, 0.001f, 0.2f);
        }
        if (pidUtilY == null) {
            pidUtilY = new MiniPID(0.04f, 0.001f, 0.2f);
        }

        pidUtilX.setOutputLimits(25);
        pidUtilY.setOutputLimits(25);
        mcontext = context;
        BaseProduct product = DJISampleApplication.getProductInstance();
        if (product != null && product.isConnected()) {
            if (product instanceof Aircraft) {
                mflightController = ((Aircraft) product).getFlightController();
                mflightController.setStateCallback(new FlightControllerState.Callback() {
                    @Override
                    public void onUpdate(FlightControllerState flightControllerState) {
                        if (flightControllerState.isLandingConfirmationNeeded()) {
                            mflightController.confirmLanding(new CommonCallbacks.CompletionCallback() {
                                @Override
                                public void onResult(DJIError djiError) {
                                    if (djiError != null)
                                        showToast(mcontext, djiError.getDescription());
                                }
                            });
                        }
                    }
                });
                /**
                 * 关闭降落保护，0.5米以下可飞行
                 */
                DJIKey enableLandingProtectionKey = FlightControllerKey.createFlightAssistantKey(FlightControllerKey.LANDING_PROTECTION_ENABLED);
                DJISDKManager.getInstance().getKeyManager().setValue(enableLandingProtectionKey, false, new SetCallback() {
                    @Override
                    public void onSuccess() {
//                        showToast(mcontext, "Disable LANDING_PROTECTION success");
                    }

                    @Override
                    public void onFailure(DJIError djiError) {
                        showToast(mcontext, djiError.getDescription());
                    }
                });
                /**
                 * Enable vision positioning.
                 * Vision positioning is used to augment GPS to improve location accuracy
                 * when hovering and improve velocity calculation when flying.
                 */
                DJIKey enableVisionAssistedPositionKey = FlightControllerKey.createFlightAssistantKey(FlightControllerKey.VISION_ASSISTED_POSITIONING_ENABLED);
                DJISDKManager.getInstance().getKeyManager().setValue(enableVisionAssistedPositionKey, true, new SetCallback() {
                    @Override
                    public void onSuccess() {
//                        showToast(mcontext, "enable VISION_ASSISTED_POSITIONING_ENABLED success");
                    }

                    @Override
                    public void onFailure(DJIError djiError) {
                        showToast(mcontext, djiError.getDescription());
                    }
                });

                /**
                 * Enables/disables precision landing.
                 * When enabled, the aircraft will record its take-off location visually (as well as with GPS).
                 * On a Return-To-Home action the aircraft will attempt to perform a precision landing using
                 * the additional visual information. This method only works on a Return-To-Home action when
                 * the home location is successfully recorded during take-off, and not changed during flight,
                 * It will take effect only after flying 10 meters high at the return point.
                 */
                DJIKey enablePrecisionlandingKey = FlightControllerKey.createFlightAssistantKey(FlightControllerKey.PRECISION_LANDING_ENABLED);
                DJISDKManager.getInstance().getKeyManager().setValue(enablePrecisionlandingKey, true, new SetCallback() {
                    @Override
                    public void onSuccess() {
//                        showToast(mcontext, "enable PRECISION_LANDING_ENABLED success");
                    }

                    @Override
                    public void onFailure(DJIError djiError) {
                        showToast(mcontext, djiError.getDescription());
                    }
                });
            }
        }
    }

    private boolean isMoving() {
        return (rX != 0) || (rY != 0) || (lX != 0) || (lY != 0);
    }

    private void stopMoving() {
        rX = rY = lX = lY = 0;
        sendVirtualSticke();
    }

    private void initSendVirtualDataTask() {
        if (null == mSendVirtualStickDataTimer) {
            mSendVirtualStickDataTask = new SendVirtualStickDataTask(mflightController);
            mSendVirtualStickDataTimer = new Timer();
            mSendVirtualStickDataTimer.schedule(mSendVirtualStickDataTask, 100, 250);
        }
    }

    public void virtualStickDataRcv(JSONObject message) {
        try {
            JSONObject virtualStickData = message.getJSONObject("msg");
            lX = virtualStickData.getInt("lX");
            rX = virtualStickData.getInt("rX");
            lY = virtualStickData.getInt("lY");
            rY = virtualStickData.getInt("rY");
            sendVirtualSticke();
        } catch (Exception e) {
            e.printStackTrace();
            Log.e("virtualStickDataRcv: ", e.toString());
        }
    }

    public void decodeControllData(String msg, float hight) {
        Log.e("decode", msg);
        curr_height = hight;
//        if (isAlternating) {
//            Log.e("isAlternating", "wait until landing finished ");
//            return;
//        }
//        if (isLanding) {
//            Log.e("isLanding", "wait until landing finished ");
//            return;
//        }
        try {
            Gson gson = new Gson();
            if (msg.equals("null")) {
                if (isMoving()) {
                    stopMoving();
                }
            } else if (isvirtualStickEnable) {
                VirtualStickData virtualStickData = gson.fromJson(msg, VirtualStickData.class);
//                rX = pidUtilX.PID_Cal(virtualStickData.getDx(), curr_height);
//                rY = pidUtilY.PID_Cal(virtualStickData.getDy(), curr_height);
//                float coe = Math.min(1,(curr_height));
//                if (curr_height < 1) coe = Math.max(0.05f,(coe * coe));
//                rX = -(float) pidUtilX.getOutput(virtualStickData.getDx() * coe,0);
//                rY = -(float) pidUtilY.getOutput(virtualStickData.getDy() * coe,0);

                if (isLanding || isAlternating) {
                    rX = rY = lX = 0;
                    lY = KP_vertical_speed * 2;
                } else {
                    rX = -(float) pidUtilX.getOutput(virtualStickData.getDx(), 0);
                    rY = -(float) pidUtilY.getOutput(virtualStickData.getDy(), 0);
                    lX = virtualStickData.getMyaw();
                    lY = vertical_speed_flitter(
//                        Math.abs(virtualStickData.getMpitch()),
//                        Math.abs(virtualStickData.getMroll()),
                            Math.abs(virtualStickData.getDx()),
                            Math.abs(virtualStickData.getDy()),
                            curr_height);
                }
                //excel操作 其他设备卡顿
//                ExcelDataUtil.writeData(virtualStickData.getDx(), virtualStickData.getDy(), rX, rY, lX, lY, curr_height, (float) pidUtilX.getI(), (float) pidUtilY.getI());
                Log.d("MiniPID--OUTPUT：", rX + "|----|" + rY + "|----|" + lX + "|----|" + lY + "|----|" + "--]" + virtualStickData.getDx() + "--]" + virtualStickData.getDy() + "--R]" + virtualStickData.getDx() + "--R]" + virtualStickData.getDy());
                sendVirtualSticke();
            }
        } catch (Exception e) {
            e.printStackTrace();
            Log.e("decodeControllData: ", e.toString());
        }
    }

    //    private float flyDownThreshold = 0.3f;
    //    private float landingThreshold = 0.12f;
    private float flyDownThreshold = 100f;
    private float landingThreshold = 20f;
    private float landingHeight = 0.4f;
    private int i = 0;

    private boolean isFlyDownOK(float... args) {
        for (float temp : args) {
            if (temp > flyDownThreshold) return false;
        }
        return true;
    }

    private float vertical_speed_flitter(float y_weight, float x_weight, float height) {

        /*
         * 垂直速度控制器
         * 当水平面 y、x 方向与停机坪降落中心点距离权重均小于 flyDownThreshold，则进入垂直速度计算模式，反之返回速度0
         * 进入垂直速度计算模式后,当水平面 y、x 方向与停机坪降落中心点距离权重均小于 landingThreshold,且高度小于landingHeight
         * 则直接执行停机，若高度大于 landingHeight，则以 KP_vertical_speed * height的速度下降
         * @param y_weight：水平面前后方向与停机坪降落中心点距离权重
         * @param x_weight：水平面左右方向与停机坪降落中心点距离权重
         * @param height：当前飞行高度
         * @param flyDownThreshold: 执行下降的水平面方向与降落中心点距离权重阈值
         * @param landingThreshold：执行停机的水平面方向与降落中心点距离权重阈值
         * @param landingHeight：执行停机的高度阈值
         * @return verticalFinalSpeed: 返回最终垂直速度
         */
//        float Threshold = 100f;
        //备降
        if (readyAlternating) {
            landingHeight = 3f;
            flyDownThreshold = 100f;
            landingThreshold = 100f;
        } else {
            //误差范围阶梯收缩
            if (height <= 5f) {
                flyDownThreshold = 75f;
                if (height <= 2f) {
                    flyDownThreshold = 35f;
                }
            }
        }
        //线性收缩
//            float a = (10f - landingHeight) * 1000f / (flyDownThreshold - landingThreshold);
//            float b = 10f * 1000 - a * flyDownThreshold;
//            float b = (landingThreshold * 10f * 1000f - flyDownThreshold * landingHeight * 1000f) / (landingThreshold - flyDownThreshold);
//            Threshold = (height - b) / a;

        Log.d("*********VSData：", y_weight + "|----|" + x_weight + "|----|" + height);
        if (y_weight < flyDownThreshold && x_weight < flyDownThreshold) {
//        if (y_weight < Threshold && x_weight < Threshold) {
            Log.d("*****VSDataDown：", y_weight + "|----|" + x_weight + "|----|" + height);
            if (y_weight <= landingThreshold && x_weight <= landingThreshold && height <= landingHeight) {
                Log.d("*****VSDataLand：", y_weight + "|----|" + x_weight + "|----|" + height);
                if (readyAlternating) {
                    isAlternating = true;
                } else {
                    //到达降落条件取消水平调整
                    rX = 0;
                    rY = 0;
                    i++;
                    //在降落条件维持一段时间再执行降落
                    if (i == 30) {
                        isLanding = true;
                    }
                }
                return 0;
            } else {
                i = 0;
                if (height > landingHeight) {
                    //下降时取消水平调整
                    rX = 0;
                    rY = 0;
                    return KP_vertical_speed * height;
                }
            }
        }
        return 0;
    }

    private void setVirtualStickSpeed(float throttle, float yaw, float pitch, float roll) {
        verticalJoyControlMaxSpeed = throttle;
        yawJoyControlMaxSpeed = yaw;
        pitchJoyControlMaxSpeed = pitch;
        rollJoyControlMaxSpeed = roll;
    }

    private void sendVirtualSticke() {
        initSendVirtualDataTask();
        Log.i("Stick", "*********" + rX + "_" + rY + "_" + lX + "_" + lY);
        //Left Stick

        /* YawControlMode = ANGLE */
        if (readyAlternating) {
            mSendVirtualStickDataTask.mYaw = landHeading;
        } else {
            /* YawControlMode = ANGULAR_VELOCITY */
            mSendVirtualStickDataTask.mYaw = yawJoyControlMaxSpeed * lX;
        }

        mSendVirtualStickDataTask.mThrottle = verticalJoyControlMaxSpeed * lY;

        //Right Stick
        mSendVirtualStickDataTask.mPitch = pitchJoyControlMaxSpeed * rX;
        mSendVirtualStickDataTask.mRoll = rollJoyControlMaxSpeed * rY;
    }

    public void enableVirtualSticks() {
        isvirtualStickEnable = true;
        pidUtilX.reset();
        pidUtilY.reset();
        if (mflightController != null) {
            initSendVirtualDataTask();
            mflightController.setVirtualStickModeEnabled(true, new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {
                    if (djiError != null) {
                        showToast(mcontext, djiError.getDescription());
                    } else {
                        mflightController.setVirtualStickAdvancedModeEnabled(true);
                        showToast(mcontext, "Enable Virtual Stick Success: " + mflightController.isVirtualStickAdvancedModeEnabled());
                        isLanding = false;
                    }
                }
            });
        }
    }

    public void disableVirtualSticks() {
        isvirtualStickEnable = false;
        if (mflightController != null) {
            mflightController.setVirtualStickModeEnabled(false, new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {
                    if (djiError != null) {
                        showToast(mcontext, djiError.getDescription());
                    } else {
                        mflightController.setVirtualStickAdvancedModeEnabled(false);
                        showToast(mcontext, "Disable Virtual Stick Success");
                    }
                }
            });
        }
    }

    public void setlX(float lX) {
        this.lX = lX;
    }

    public void setlY(float lY) {
        this.lY = lY;
        sendVirtualSticke();
    }

    public void Aircraft_takeoff() {
        if (mflightController != null) {
            mflightController.startTakeoff(new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {
                    if (djiError != null) {
                        showToast(mcontext, djiError.getDescription());
                    } else {
                        showToast(mcontext, "Take off Success");
                        isLanding = false;
                    }
                }
            });
        } else {
            showToast(mcontext, "flightcontroller null");
        }
    }

    private void startRotation_mission() {
        initSendVirtualDataTask();
        mflightController.setVirtualStickModeEnabled(true, new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (djiError != null) {
                    showToast(mcontext, djiError.getDescription());
                } else {
                    showToast(mcontext, "Enable Virtual Stick Success");
                }
            }
        });
        mflightController.setVerticalControlMode(VerticalControlMode.POSITION);
        lY = 40;
        lX = 1.5f;
        sendVirtualSticke();
        Rotation.Builder builder = new Rotation.Builder().mode(RotationMode.ABSOLUTE_ANGLE).time(2);
        builder.pitch(-50f);
        DJIKey gimbalRotateKey = GimbalKey.create(GimbalKey.ROTATE);
        DJISDKManager.getInstance().getKeyManager().performAction(gimbalRotateKey, new ActionCallback() {
            @Override
            public void onSuccess() {
                Log.i("Rotate Gimbal", "onSuccess: ");
            }

            @Override
            public void onFailure(DJIError djiError) {
            }
        }, builder);
    }

    public void stopRotation_mission() {
        mflightController.setVerticalControlMode(VerticalControlMode.VELOCITY);
        lY = 0;
        lX = 0;
        sendVirtualSticke();
    }

    public void Aircraft_land() {
        if (mflightController != null) {
            mflightController.startLanding(
                    new CommonCallbacks.CompletionCallback() {
                        @Override
                        public void onResult(DJIError djiError) {
                            if (djiError != null) {
                                showToast(mcontext, djiError.getDescription());
                            } else {
                                showToast(mcontext, "Start Landing");
                                isLanding = false;
                                isAlternating = false;
                                disableVirtualSticks();
                            }
                        }
                    }
            );
        } else {
            showToast(mcontext, "flightcontroller null");
        }
    }

    public void closeVirtualStickController() {
        if (null != mSendVirtualStickDataTimer) {
            mSendVirtualStickDataTask.cancel();
            mSendVirtualStickDataTask = null;
            mSendVirtualStickDataTimer.cancel();
            mSendVirtualStickDataTimer.purge();
            mSendVirtualStickDataTimer = null;
        }
    }

    public void setKPID_Dialog() {
        LayoutInflater inflater = LayoutInflater.from(mcontext);
        View vv = inflater.inflate(R.layout.make_kpid_dialog, null);
        final EditText editText_kp = vv.findViewById(R.id.KP);
        final EditText editText_ki = vv.findViewById(R.id.KI);
        final EditText editText_kd = vv.findViewById(R.id.KD);

        editText_kp.setText(String.valueOf(pidUtilX.getP()));
        editText_ki.setText(String.valueOf(pidUtilX.getI()));
        editText_kd.setText(String.valueOf(pidUtilX.getD()));

        new AlertDialog.Builder(mcontext, R.style.jzxz_dialog)
                .setView(vv)
                .setPositiveButton("确定", new DialogInterface.OnClickListener() {
                    @Override
                    public void onClick(DialogInterface dialog, int which) {
                        if (!editText_kp.getText().toString().equals("") && !editText_ki.getText().toString().equals("") && !editText_kd.getText().toString().equals("")) {
                            float value_kp = Float.parseFloat(editText_kp.getText().toString());
                            float value_ki = Float.parseFloat(editText_ki.getText().toString());
                            float value_kd = Float.parseFloat(editText_kd.getText().toString());

                            pidUtilX.setP(value_kp);
                            pidUtilX.setI(value_ki);
                            pidUtilX.setD(value_kd);
                            pidUtilY.setP(value_kp);
                            pidUtilY.setI(value_ki);
                            pidUtilY.setD(value_kd);
                        }
                        dialog.dismiss();
                    }
                })
                .create()
                .show();
    }


    /**
     * 精准起飞，执行后无人机起飞到6米，记录当前GPS，下视摄像头记录周围环境
     *
     * @param mlandHeading 记录起飞前方向角，用于返航停机时进行校准
     */
    public void Aircraft_PrecisionTakeoff(float mlandHeading) {
        landHeading = mlandHeading;
        showToast(mcontext, "landHeading:" + landHeading);
        if (mflightController != null) {
            mflightController.startPrecisionTakeoff(djiError -> {
                if (djiError != null) {
                    showToast(mcontext, djiError.getDescription());
                } else {
                    showToast(mcontext, "Take off Success");
                    isLanding = false;
                }
            });
        } else {
            showToast(mcontext, "flightcontroller null");
        }
    }
}
